/*
  File:    Matrix.cpp
  Purpose: To provide an easy-to-use matrix object for 
           passing data, as well as reading/writing 
		   from/to files. Many common Matlab-like 
		   functions are provided.

  Contact: Paul Macklin
           pmacklin@math.uci.edu
		   http://math.uci.edu/~pmacklin
*/

#include <cstdlib>
#include <cstdio>
#include <cmath>
#include <iostream>
#include <cstring>
#include <cctype>

using namespace std;

extern double _EPS;

#include "Matrix.h"
#include "ImprovedMath.h"
#include "Endian.h"

Matrix::Matrix()
{
 Rows = 1;
 Cols = 1;
 Data = new double* [Rows];
 Data[0] = new double [Cols];
 
 Data[0][0] = 0.0;
 
 return;
}

Matrix::Matrix( int NewRows , int NewCols )
{
 Rows = 0;
 Cols = 0;
 SetSize(NewRows,NewCols);
}

void Matrix::SetSize( int NewRows, int NewCols )
{
 int i,j;
 
 if( Rows > 0 && Cols > 0 )
 {
  for( i=0 ; i < Rows ; i++ )
  {
   delete [] Data[i]; 
  }
  delete [] Data;
 }
 
 Data = new double* [NewRows];
 for( i=0 ; i < NewRows ; i++ )
 {
  Data[i] = new double [NewCols];
 }

 for( i=0 ; i < NewRows ; i++ )
 {
  for( j=0 ; j < NewCols ; j++ )
  { Data[i][j] = 0.0; }
 }
 Rows = NewRows;
 Cols = NewCols;
 
 return;
}

int Matrix::TellRows( void ) const
{ return Rows; }
int Matrix::TellCols( void ) const
{ return Cols; }

void Matrix::display( void )
{
 int i,j;
 cout.precision( 12 );
 for( i=0 ; i < Rows ; i++ )
 {
  for( j=0 ; j < Cols ; j++ )
  { cout << Data[i][j] << " "; }
  cout << endl;
 }
 return;
}

double* Matrix::operator()( int i, int j ) const
{
 if( i >= Rows || j >= Cols )
 {
  cout << "\tError! Attempted to access nonexistent matrix entry " << i << "," << j << " for matrix at address " << this << endl;
  i=0; j=0; 
 }
 return &(Data[i][j]);
}

Matrix Matrix::operator+( const Matrix& B )
{
 if( Rows != B.TellRows() || Cols != B.TellCols() )
 { 
  cout << "\tError! Attempted to add matrices of unequal size." << endl;
  return B;
 }
 
 Matrix Output( B.TellRows() , B.TellCols() );
 int i,j;
 
 for( i=0 ; i < Rows ; i++ )
 {
  for( j=0 ; j < Cols ; j++ )
  { *Output(i,j) = Data[i][j] + *B(i,j); }
 }
 
 return Output;
}

Matrix::~Matrix()
{
 int i;
 for( i=0 ; i < Rows ; i++ )
 { delete [] Data[i]; }
 delete [] Data;
}

Matrix& Matrix::operator=( const Matrix& B )
{
 SetSize( B.TellRows() , B.TellCols() ); 
 
 int i,j;
 for( i=0 ; i < Rows ; i++ )
 {
  for( j=0 ; j < Cols ; j++ )
  {
   Data[i][j] = *B(i,j);
  }
 }

 return *this;
}

Matrix::Matrix( const Matrix& Input )
{
 Rows = -1;
 Cols = -1;
 SetSize( Input.TellRows(), Input.TellCols() );
 int i,j;
 for( i=0 ; i < Rows ; i++ )
 {
  for( j=0 ; j < Cols ; j++ )
  {
   Data[i][j] = *Input(i,j);  
  }
 }
 return;
}

Matrix Matrix::operator-( const Matrix& Input )
{
 if( Rows != Input.TellRows() || Cols != Input.TellCols() )
 { 
  cout << "\tError! Attempted to subtract matrices of unequal size." << endl;
  return Input;
 }

 Matrix Output( Input.TellRows() , Input.TellCols() );
 
 int i,j;
 for( i=0 ; i < Rows ; i++ )
 {
  for( j=0 ; j < Cols ; j++ )
  {
   *Output(i,j) = Data[i][j] - *Input(i,j);   
  }
 }
 
 return Output;
}

double Matrix::InfinityNorm( void )
{
 int i,j;
 double Max = -1;
 for( i=0 ; i < Rows ; i++ )
 {
  for( j=0 ; j < Cols ; j++ )
  {
   double Temp = fabs( Data[i][j] );
   if( Temp > Max )
   { Max = Temp; }
  }
 }
 return Max;
}

double Matrix::Pnorm( double p )
{
 if( p <= 0 )
 { 
  cout << "Error! Requested nonsensical " << p << " norm of matrix!" << endl;
  return 0;
 }

 double Output = 0.0;
 int i,j;
 for( i=0 ; i < Rows ; i++ )
 {
  for( j=0 ; j < Cols ; j++ )
  {
   Output += pow( Data[i][j] , p );
  }
 }
 return pow( Output, 1.0/(double) p );
}

double Matrix::IntPnorm( int p )
{
 if( p <= 0 )
 { 
  cout << "Error! Requested nonsensical " << p << " norm of matrix!" << endl;
  return 0;
 }

 double Output = 0.0;
 int i,j;
 for( i=0 ; i < Rows ; i++ )
 {
  for( j=0 ; j < Cols ; j++ )
  {
   Output += IntPow( Data[i][j] , p );
  }
 }
 return pow( Output, 1.0/(double) p );
}

Matrix zeros( int NewRows, int NewCols )
{
 Matrix Output(NewRows,NewCols);
 return Output;
}

Matrix ones( int NewRows, int NewCols )
{
 Matrix Output(NewRows,NewCols);
 int i,j;
 for( i=0 ; i < Output.TellRows() ; i++ )
 {
  for( j=0 ; j < Output.TellCols() ; j++ )
  { *Output(i,j) = 1.0; }
 }
 return Output;
}

bool IsDelimiter( char c )
{ 
 if( c == ' ' || c == '\t' || c == ',' || c == ';' )
 { return true; }
 return false;
} 

int CountColumns( const char* FileName , char Delimiter )
{
 int output = 0;
 FILE* fp;
 fp = fopen( FileName , "r" );
 if( !fp ) 
 { return output; }

 bool done = false;
 
 // eat any delimiters at the beginning of the file 
 // I have to do this to deal with programs that 
 // insert extra whitespace. 
 while( !done && !feof( fp ) )
 {
  char c = getc(fp);
//  if( c != ','  && c != ' ' &&
//      c != '\t' && c != ';' )
  if( !IsDelimiter( c ) )
  { done = true; ungetc( c , fp ); }
 }
 done = false;
 
 bool CharFound = false;
 
 while( !done && !feof( fp ) )
 {
  char c = getc(fp);
  
  // stop when we reach what must be a newline
  if( c <= 8 )
  { done = true; }
  if( c >= 10 && c <= 31 )
  { done = true; }

  if( isdigit(c) || c == '-' || c == 'e' )
  { CharFound = true; }
  if( CharFound && c == Delimiter )
  { CharFound = false; output++; }
 }
 
 if( CharFound == true )
 { output++; }
 
 fclose( fp );
 return output;
}

int CountRows( const char* FileName , char Delimiter )
{
 int output = 0;
 FILE* fp;
 fp = fopen( FileName , "r" );
 if( !fp ) 
 { return output; }

 bool really_done = false;
 
 while( !really_done && !feof( fp ) )
 {
  bool done = false; 
  bool MeaningfulRow = false;
  while( !done && !feof( fp ) )
  {
   char c = getc(fp);
   // stop when we reach what must be a newline
   if( c <= 8 )
   { done = true; }
   if( c >= 10 && c <= 31 )
   { done = true; }
   if( c == Delimiter || isdigit(c) || c == '.' || c == 'e' ) 
   // added isdigit and so forth
   { MeaningfulRow = true; }
  }
  if( MeaningfulRow )
  { output++; }
 }
 
 fclose( fp );
 return output;
}

// returns the next double in the file buffer

double GetDoubleFromFILE( FILE* fp , char Delimiter )
{
 char Output[2050];
 int Position = 0;
 bool done = false;
 
 // eat whitespace, preceding delimiters, and funny crap 
 
 while( !feof( fp ) && Position < 2049 && !done )
 {
  char c = getc( fp );
  bool internalstop = true;
  if( c <= 8 )
  { internalstop = false; }
  if( c >= 10 && c <= 31 )
  { internalstop = false; }  
  if( c == Delimiter )
  { internalstop = false; }
  if( internalstop )
  { ungetc( c, fp ); done = true; }
 } 
 
 // get all non-delimiter, non-weird characters before next delimiter
 done = false;
 while( !feof( fp ) && Position < 2049 && !done )
 {
  char c = getc( fp );
  if( c == Delimiter )
  { done = true; ungetc( c , fp ); }
  if( c <= 8 )
  { done = true; ungetc( c , fp ); }
  if( c >= 10 && c <= 31 )
  { done = true; ungetc( c , fp ); }
  if( !done )
  { Output[Position] = c; Position++; }
 }

 // insert null character 
 Output[Position] = '\0';
 
 return strtod( Output, NULL );
}

char DetermineDelimiter( const char* FileName )
{
 char output = '0';
 FILE* fp;
 fp = fopen( FileName , "r" );
 if( !fp ) 
 { return output; }
  
 bool done = false;
 
 // eat any delimiters at the beginning of the file 
 // I have to do this to deal with programs that insert extra whitespace. 
 while( !done && !feof( fp ) )
 {
  char c = getc(fp);
//  if( c != ',' && c != ' ' &&
//      c != '\t'&& c != ';' )
  if( !IsDelimiter( c ) )
  { done = true; ungetc( c , fp ); }
 }
 done = false;

 // now search for the first nontrivial delimiter 
 while( !done && !feof( fp ) )
 {
  char c = fgetc(fp);
//  if( c == ' ' || 
//      c == ' ' || 
//      c == '\t' ||
//      c == ';' || 
//	  c == '\n' ) // added the possibility of newline
  if( IsDelimiter( c ) || c == '\n' )
  { 
   done = true; 
   output = c;
  }
 }
 fclose( fp );  
 // do this just to make it work a tad bit better 
 // ideally, we should fix the code that looks for 
 // nontrivial lines
 if( output == '\n' )
 { output = ' '; }
 
 return output;
}

bool Matrix::ReadFromTextFile( const char* FileName )
{
 char Delimiter = DetermineDelimiter( FileName );
 int NewRows = CountRows( FileName , Delimiter );
 int NewCols = CountColumns( FileName , Delimiter );

 FILE* fp;
 fp = fopen( FileName, "r" );
 if( !fp )
 { return false; }
 
 SetSize( NewRows , NewCols );
 int i,j;
 for( i=0 ; i < Rows ; i++ )
 {
  for( j=0 ; j < Cols ; j++ )
  {
   Data[i][j] = GetDoubleFromFILE( fp , Delimiter );
  }
 }
 
 fclose( fp );
 return true;
}

bool Matrix::ReadFromFile( const char* FileName )
{
 bool TextReadMode = true;
 
 // determine that it's a BDF file
 
 FILE* fp;
 fp = fopen( FileName , "rb" );
 if( !fp )
 { return false; }
 char c1 = getc(fp); 
 char c2 = getc(fp);
 char c3 = getc(fp);
 if( c1 == 'B' && c2 == 'D' && c3 == 'F' )
 { TextReadMode = false; }

 if( TextReadMode )
 {
  fclose( fp );
  return ReadFromTextFile( FileName );
 }

 // read the size of a double. 
 
 char DataSize;
 fread( &DataSize, 1, 1 , fp );
 if( DataSize != 8 )
 {
  cout << "Error: Reading double sizes other than 8" << endl
       << "       is currently not supported." << endl;
  SetSize(1,1);
  fclose( fp );
  return false; 
 }
 int IntegerDataSize = (int) DataSize; 
 
 bool BigE = BigEndian();
 
 double d;
 if( BigE )
 { d = ReadDoubleOnBigEndianMachine( fp ); }
 else
 { fread( (char*) &d , 1 , IntegerDataSize , fp ); }
 int NewRows = (int) d;
 
 if( BigE )
 { d = ReadDoubleOnBigEndianMachine( fp ); }
 else
 { fread( (char*) &d , 1 , IntegerDataSize , fp ); }
 int NewCols = (int) d;

 SetSize( NewRows , NewCols );
 
 int i,j;
 
 if( BigE )
 {
  for( i=0 ; i < Rows ; i++ )
  {
   for( j=0 ; j < Cols ; j++ )
   {
    if( !feof( fp ) )
    {
     Data[i][j] = ReadDoubleOnBigEndianMachine( fp );
    }
   }
  }
 }
 else
 { 
  for( i=0 ; i < Rows ; i++ )
  {
   for( j=0 ; j < Cols ; j++ )
   {
    if( !feof( fp ) )
    {
     fread( (char*) &(Data[i][j]) , 1 , IntegerDataSize , fp );
    }
   }
  }
 } 
 fclose( fp );
 
 return true;
}

bool Matrix::WriteToFile( const char* FileName )
{
 FILE* fp;
 fp = fopen( FileName , "wb" );
 if( !fp )
 { return false; }

 bool BigE = BigEndian();

 // write the string "BDF" to the start of the file 
 // to indicate the "binary data format"
 
 char c = 'B';
 fwrite( &c , 1 , 1 , fp );
 c = 'D';
 fwrite( &c , 1 , 1 , fp );
 c = 'F';
 fwrite( &c , 1 , 1 , fp );
 
 // Write the size of the data field (usually 8 bytes for 
 // doubles) here. This is mostly for future-proofing the 
 // format.
 
 char DataSize = (char) sizeof( double );
 int IntegerDataSize = (int) DataSize;
 
 fwrite( &DataSize , 1 , 1 , fp );
 
 if( DataSize != 8 && BigE )
 {
  cout << "Error: Unable to write greater than 8-bit fields" << endl
       << "       on big-endian machines at this time!" << endl;
  fclose( fp );
  return false; 
 }
 
 // write the row and columns sizes 
 
 double DoubleRows = (double) Rows; 
 double DoubleColumns = (double) Cols;
 
 if( BigE )
 {
  WriteDoubleOnBigEndianMachine( fp , DoubleRows );
  WriteDoubleOnBigEndianMachine( fp , DoubleColumns );
 }
 else
 {
  fwrite( (char*) &DoubleRows , 1 , IntegerDataSize , fp );
  fwrite( (char*) &DoubleColumns , 1 , IntegerDataSize , fp );
 }
 
 if( BigE )
 {
  int i,j;
  for( i=0 ; i < Rows ; i++ )
  {
   for( j=0 ; j < Cols ; j++ )
   { WriteDoubleOnBigEndianMachine( fp , Data[i][j] ); }
  }
 }
 else
 {
  int i,j;
  for( i=0 ; i < Rows ; i++ )
  {
   for( j=0 ; j < Cols ; j++ )
   { fwrite( (char*) &(Data[i][j]) , 1, DataSize, fp ); }
  }
 }
 fclose( fp );

 return true;
}

bool Matrix::WriteToTextFile( const char* FileName )
{
 FILE* fp;
 fp = fopen( FileName , "w" );
 if( !fp )
 { return false; }
 
 int i,j,k;
 for( i=0 ; i < Rows ; i++ )
 {
  for( j=0 ; j < Cols ; j++ )
  {
   char StringBuffer [1024];
   sprintf(StringBuffer , "%1.16e ", Data[i][j] ); 
   for( k=0 ; k < strlen( StringBuffer ) ; k++ )
   { fwrite( StringBuffer+k , 1 , 1 , fp ); }
  }
  char c = '\n';
  fwrite( &c , 1 , 1 , fp );
 }
 fclose( fp );
 return true;
}

double* Matrix::operator()( int j ) const
{
 if( j >= Cols )
 {
  cout << "\tError! Attempted to access nonexistent matrix entry." << endl;
  j=0;
 }
 return &(Data[0][j]);
}

/*
Matrix operator*( double d , Matrix& Input ) 
{
 Matrix Output = Input;
 int i,j;
 for( i=0 ; i < Output.TellRows() ; i++ )
 {
  for( j=0 ; j < Output.TellCols() ; j++ )
  { *Output(i,j) *= d;  }
 }
 return Output;
}
*/

Matrix operator*( double d, Matrix Input2)
{
 Matrix Output = Input2;
 int i,j;
 for( i=0 ; i < Output.TellRows() ; i++ )
 {
  for( j=0 ; j < Output.TellCols() ; j++ )
  { *Output(i,j) *= d;  }
 }
 return Output;
}

Matrix Matrix::operator*( Matrix &Input2)
{
 // In this case, it's actually matrix-vector 
 // multiplication. We store the matrix "wrong"
 // for improved memory performance. (All data in 
 // contiguous memory.)
 
 if( Input2.TellRows() == 1 )
 {
  if( Cols != Input2.TellCols() )
  { 
   cout << "\tError! Improper attempt at matrix-vector multiplication!" << endl;
   return Input2;
  }
 
  Matrix Output = zeros( 1, Rows );
  int i;
  for( i=0 ; i < Rows ; i++ )
  {
   double Sum = 0.0;
   for( int j = 0 ; j < Cols ; j++ )
   { Sum += Data[i][j] * *Input2(j); }
   *Output(i) = Sum; 
  }
  return Output; 
 }

 if( Cols != Input2.TellRows() )
 { 
  cout << "\tError! Improper attempt at matrix-matrix multiplication!" << endl;
  return Input2;
 }

 Matrix Output = zeros( Rows , Input2.TellCols() );
 int i,j;
 for( i=0 ; i < Output.TellRows() ; i++ )
 {
  for( j=0 ; j < Output.TellCols() ; j++ )
  {
   double Sum = 0.0;
   for( int k=0 ; k < Cols ; k++ )
   { Sum += Data[i][k] * *Input2(k,j); }
   *Output(i,j) = Sum;   
  }
 } 
 return Output;
}

Matrix Matrix::submatrix( int StartRow, int EndRow, 
                          int StartCol, int EndCol )
{
 Matrix Output = zeros( EndRow-StartRow+1 , EndCol-StartCol+1 );
 
 int i,j;
 for( i=0 ; i < Output.TellRows() ; i++ )
 {
  for( j=0; j < Output.TellCols() ; j++ )
  {
   *Output(i,j) = Data[StartRow+i][StartCol+j];
  }
 }

 return Output;
}

// double-check this to make sure we have it matching the old

void Matrix::UpdateSubmatrix( Matrix& Input, int StartRow, int EndRow, 
                                             int StartCol, int EndCol )
{
 int i,j;
 for( i=0 ; i < Input.TellRows() ; i++ )
 {
  for( j=0 ; j < Input.TellCols() ; j++ )
  { Data[StartRow+i][StartCol+j] = *Input(i,j); }
 }

 return;
}

double DotProduct(Matrix &In1 , int Row1, int StartCol1 ,
                  Matrix &In2 , int Row2, int StartCol2 , 
                  int NumberOfSummands)
{
 double Output = 0.0;
 int j = 1;
 for(j = 1 ; j <= NumberOfSummands ; j++)
 {
  Output = Output + ( (*In1(Row1,StartCol1-1+j)) * 
                      (*In2(Row2,StartCol2-1+j)) );
 }
 return Output;
}

Matrix GaussianEliminationPartialPivoting( 
        Matrix &Coefficient, Matrix &RHS )
{
 double _EPS = 2.2e-16;

 if( Coefficient.TellRows() != Coefficient.TellCols() )
 {
  cout << "Error in Gaussian Elimination: Matrix must be square! \n";
  return RHS;
 }
 if( RHS.TellCols() != Coefficient.TellRows() )
 { 
  cout << "Error in Gaussian Elimination: RHS must have same number \n"
       << "                               of entries as rows in Coefficient Matrix\n";
  return RHS;
 }
 int i,j,k;
 
 Matrix Out = zeros(1,RHS.TellCols());
 int n = Coefficient.TellCols();
 
 // prepare the augmented matrix
 Matrix Augmented = zeros(n,n+1);
 Augmented.UpdateSubmatrix( Coefficient, 0,Coefficient.TellRows()-1,0,Coefficient.TellCols()-1);
 
 for( i=0 ; i < RHS.TellCols() ; i++)
 {
  *Augmented(i,n) = *RHS(i);
 }
 
 // initialize the row pointer ; Step 1
 int* RowPointers;
 RowPointers = new int [n];
 for( i=0 ; i < n ; i++)
 { RowPointers[i] = i; }
 Matrix Multipliers = zeros(n,n);
 
 // Elimination process ; Step 2
 for( i=0 ; i < n-1 ; i++)
 {
 
  // Step 3
  int p = i;
  bool Pfound = false;
  while( !Pfound && p < n-1)
  {
   double Value = fabs( *Augmented( RowPointers[p] , i ) );
   Pfound = true;
   for( j=i ; j < n ; j++)
   {
    if( fabs( *Augmented(RowPointers[j],i) ) > Value )
    {
     Pfound = false;
    }
   }
   if( !Pfound )
   {
    p++;
   }
  }
  // p is now found, and is between i and n-1
  
  // Step 4
  if( fabs( *Augmented(RowPointers[p],i) ) <= _EPS )
  {
//   cout << "Error!\n";
   return RHS;
  }
  
  // Step 5
  if( RowPointers[i] != RowPointers[p] )
  {
   int temp = RowPointers[i];
   RowPointers[i] = RowPointers[p];
   RowPointers[p] = temp;
  }
  
  // Step 6
  for( j=i+1; j < n ; j++)
  {
   // Step 7
   *Multipliers(RowPointers[j],i) = *Augmented(RowPointers[j],i) / *Augmented(RowPointers[i],i);
   
   // Step 8
   Matrix Temp0 = Augmented.submatrix(RowPointers[j],RowPointers[j],0,Augmented.TellCols()-1);
   Matrix Temp1 = Augmented.submatrix(RowPointers[i],RowPointers[i],0,Augmented.TellCols()-1);
//   Matrix Temp1a = (*Multipliers(RowPointers[j],i))*Temp1;
//   Matrix Temp2 = Temp0 - Temp1a;
   Matrix Temp2 = Temp0 - (*Multipliers(RowPointers[j],i))*Temp1;
   Augmented.UpdateSubmatrix(Temp2,RowPointers[j],RowPointers[j],0,Augmented.TellCols()-1);
  } 
 }
 // Step 9
 if( fabs( *Augmented(RowPointers[n-1],n-1) ) <= _EPS )
 {
//  cout << "Error! (Division by zero in back-substitution)\n";
  return RHS;
 }
 
 // Step 10
 *Out(n-1) = *Augmented(RowPointers[n-1],n) / *Augmented(RowPointers[n-1],n-1);
 
 // Step 11
 for( i=n-2 ; i >= 0 ; i-- )
 {
  double TempSum = 0.0;
  for( j=i+1 ; j < n ; j++)
  {
   TempSum += ( *Augmented(RowPointers[i],j) )*( *Out(j) );
  }
  *Out(i) = ( *Augmented( RowPointers[i],n) - TempSum ) / *Augmented(RowPointers[i],i); 
 }
 
 // Step 12
 delete [] RowPointers;
 return Out;
}

Matrix Transpose( Matrix &Input )
{
 Matrix Output( Input.TellCols() , Input.TellRows() );
 int i,j;
 for( j=0 ; j < Output.TellCols() ; j++ )
 {
  for( i=0 ; i < Output.TellRows() ; i++ )
  { *Output(i,j) = *Input(j,i); }
 }
 return Output;
}

Matrix linspace( double left, double increment, double right )
{
 int size = ImprovedRound( (right-left)/increment ) + 1;  
 Matrix Output = zeros(1,size);
 int i;
 for( i=0 ; i < Output.TellCols() ; i++ )
 { *Output( i ) = left + i*increment; }

 return Output;
}

Matrix linspace( double left, double right, int size )
{
 long double increment = ( ((long double)right-(long double)left)/( (long double) size-1.0) );
// printf( "increment: %3.25f\n", increment );
// increment = (double) ImprovedRound( 10000 * increment ) / 10000.0;
// printf( "increment: %3.25f\n", increment );
 
 Matrix Output = zeros(1,size);

 int i;
 long double total = (long double) left;
 for( i=0 ; i < Output.TellCols() ; i++ )
 {
//  *Output( i ) = (double) ((long double)left + increment*(long double)i );
  *Output(i) = (double) total;
  total += increment;
 }
 cout << endl;
 return Output;
}

double DotProduct( Matrix& Input1 , Matrix& Input2 )
{
 double output = 0.0;
 int j = 0;
 for( j=0 ; j < Input1.TellCols() ; j++ )
 { output += (*Input1(0,j))*(*Input2(0,j)); }
 return output;
}
Matrix CrossProduct( Matrix& Input1, Matrix& Input2 )
{
 Matrix Output = zeros(1,3);
 *Output(0) = (*Input1(1))*(*Input2(2)) - (*Input1(2))*(*Input2(1));
 *Output(1) = (*Input1(2))*(*Input2(0)) - (*Input1(0))*(*Input2(2));
 *Output(2) = (*Input1(0))*(*Input2(1)) - (*Input1(1))*(*Input2(0));
 return Output;
}

Matrix linsolve2by2( Matrix& A, Matrix& b )
{
 Matrix Output = zeros(1,2);
 double det = *A(0,0) * *A(1,1) - *A(1,0) * *A(0,1);
 if( fabs( det ) <= _EPS )
 {
  cout << "linsolve2by2 error: System has zero determinant!" << endl;
  return Output;
 }
 *Output(0) = ( *A(1,1) * *b(0) - *A(0,1) * *b(1) )/det;
 *Output(1) = ( *A(0,0) * *b(1) - *A(1,0) * *b(0) )/det;
 return Output; 
}
